#include "cascadedefin.h"
#include "motion.h"
#include "videofacedetect.h"
#include <string.h>
#include "math.h"
#include "stdio.h"
#include <sys/time.h>
#include <time.h>

FaceBuff* createfacedetect(int width, int height)
{
	FaceBuff *pf = (FaceBuff*) malloc(sizeof(FaceBuff));
	if (pf == NULL) {
		return NULL;
	}
	//16bite bk
	pf->background16 = (int*) malloc(sizeof(int) * width * height);
	//8bite bk
	pf->background8 = (unsigned char*) malloc(
			sizeof(unsigned char) * width * height);
	//inter
	pf->inter = (int*) malloc(sizeof(int) * (width + 1) * (height + 1));
	pf->squreinter = (int*) malloc(
			sizeof(int) * (width + 1) * (height + 1));
	pf->motionbuf = (unsigned char*) malloc(
			sizeof(unsigned char) * width * height);
	pf->motioninter = (int*) malloc(
			sizeof(int) * (width + 1) * (height + 1));
	pf->motionmask = (unsigned char*) malloc(
			sizeof(unsigned char) * width * height);

	return pf;
}
void downsample(unsigned char* src, int s_step, unsigned char* dst, int d_w,
		int d_h, int downsacle)
{
	int x, y;
	unsigned char* ps, *pd;
	for (y = 0; y < d_h; y++) {
		ps = src + y * downsacle * s_step;
		pd = dst + y * d_w;
		for (x = 0; x < d_w; x++) {
			pd[x] = ps[downsacle * x];
		}
	}
}
void learnbk(int *bk, unsigned char* frame, unsigned char* bk8, int w, int h,
		int widthstep, float learnrate)
{
	int x, y;
	int *pb;
	unsigned char* pf;
	unsigned char* pb8;
	int b = 0;
	for (y = 0; y < h; y++) {
		pb = bk + y * w;
		pf = frame + y * widthstep;
		pb8 = bk8 + y * w;
		for (x = 0; x < w; x++) {
			pb[x] = pb[x] * (1 - learnrate)
					+ (((int) pf[x]) << 10) * learnrate;
			pb8[x] = pb[x] >> 10;
		}
	}
}
void bkfirst(int *bk16, unsigned char* frame, int w, int h, int widthstep)
{
	int x, y;
	int *pb;
	unsigned char* pf;
	for (y = 0; y < h; y++) {
		pb = bk16 + y * w;
		pf = frame + y * widthstep;
		for (x = 0; x < w; x++) {
			pb[x] = ((int) pf[x]) << 10;
		}
	}
}

static void integral_buf(unsigned char* pSrcImg, int *pSum, int W, int H)
{
	int x, y;
	int s = 0;
	int srcstep = W;
	int sumstep = W + 1;
	unsigned char *src = pSrcImg;
	int *sum = pSum;

	memset(sum, 0, (W + 1) * sizeof(sum[0]));
	sum += sumstep + 1;
	sum[-1] = 0;

	for (x = 0, s = 0; x < W; x++) {
		s += src[x];
		sum[x] = s;
	}

	for (y = 1; y < H; y++) {
		src += srcstep;
		sum += sumstep;

		s = src[0];
		sum[-1] = 0;
		sum[0] = sum[-sumstep] + s;

		for (x = 1; x < W; x++) {
			s += src[x];
			sum[x] = sum[x - sumstep] + s;
		}
	}
}
void drawmask(RECT res[], int nums, unsigned char* mask, int* maskinter, int w,
		int h)
{
	int i = 0;
	memset(mask, 0, sizeof(unsigned char) * w * h);
	for (i = 0; i < nums; i++) {
		int x, y;
		unsigned char* ps = NULL;
		for (y = res[i].top; y < res[i].bottom; y++) {
			ps = mask + y * w;
			for (x = res[i].left; x < res[i].right; x++) {
				ps[x] = 1;
			}
		}
	}
	//计算mask积分图
	integral_buf(mask, maskinter, w, h);
}

//计算相似度
float kzTempmatch(unsigned char* imageA, unsigned char *imageB, RECT pos,
		int step_A, int step_B)
{
	int h, w;
	float mean_I = 0.0, mean_T = 0.0;
	float sum_I = 0.0, sum_T = 0.0, IxIx = 0.0, TxTx = 0.0;
	float Ix, Tx;
	float size = (pos.right - pos.left) * (pos.bottom - pos.top);
	float sum = 0.0;
	unsigned char* pSrc = NULL;
	unsigned char* pDst = NULL;

	for (h = pos.top; h < pos.bottom; h++) {
		pSrc = imageA + h * step_A;
		pDst = imageB + h * step_B;
		for (w = pos.left; w < pos.right; w++) {
			Ix = pSrc[w];
			Tx = pDst[w];

			sum_I += Ix;
			sum_T += Tx;
		}
	}

	mean_I = sum_I / size;
	mean_T = sum_T / size;

	for (h = pos.top; h < pos.bottom; h++) {
		pSrc = imageA + h * step_A;
		pDst = imageB + h * step_B;
		for (w = pos.left; w < pos.right; w++) {
			Ix = pSrc[w];
			Tx = pDst[w];
			sum += (Ix - mean_I) * (Tx - mean_T);
			IxIx += (Ix - mean_I) * (Ix - mean_I);
			TxTx += (Tx - mean_T) * (Tx - mean_T);
		}
	}
	return sum / sqrt(IxIx * TxTx);
}

int videofacedetect(unsigned char* grayimage, int width, int height,
		int widthstep, Kise_Rect face[50], int MinSize, int MaxSize,
		float scale, FaceBuff *pf)
{
	int motion_w, motion_h;
	const int downscale = 2;
	static int frame = 0;
	int movenums = 0;
	RECT motionres[50];
	int i = 0;
	objPos facetemp[50];
	int facecounts = 0, fs = 0;

//	struct timeval begin, end;
//	int time_detect = 0;

	//背景更新
	if (frame % 1 == 0) {
		printf("\t\t[1]background....\n");
		if (frame == 1) {
			memset(pf->background8, 0,
					sizeof(unsigned char) * width * height);
			bkfirst(pf->background16, grayimage, width, height,
					widthstep);
		} else {
//			gettimeofday(&begin, NULL);
			learnbk(pf->background16, grayimage, pf->background8,
					width, height, widthstep, 0.01);
//			gettimeofday(&end, NULL);
//			time_detect = (end.tv_sec - begin.tv_sec) * 1000000
//					+ (end.tv_usec - begin.tv_usec);
//			printf("\t\t learnbk time : %d\n", time_detect);
		}
	}
	if (frame % 1 == 0) {
		printf("\t\t[2]motion....\n");
		motion_w = width / downscale;
		motion_h = height / downscale;

//		gettimeofday(&begin, NULL);
		//下采样
		downsample(grayimage, widthstep, pf->motionbuf, motion_w,
				motion_h, downscale);

		//进行动检
		if (frame == 0) {
			initmotion(motion_w, motion_h);
		}

		motiondetect(pf->motionbuf, motion_w, motion_h, 0, motion_w, 0,
				motion_h, 1, motionres, &movenums);

//		gettimeofday(&end, NULL);
//		time_detect = (end.tv_sec - begin.tv_sec) * 1000000
//				+ (end.tv_usec - begin.tv_usec);
//		printf("\t\t motiondetect time : %d\n", time_detect);

		//resize rect
		for (i = 0; i < movenums; i++) {
			motionres[i].left *= downscale;
			motionres[i].right *= downscale;
			motionres[i].top *= downscale;
			motionres[i].bottom *= downscale;
		}
		if (movenums > 0) {
			//计算mask积分图,为后续区域检测做准备
//			gettimeofday(&begin, NULL);

			drawmask(motionres, movenums, pf->motionmask,
					pf->motioninter, width, height);

//			gettimeofday(&end, NULL);
//			time_detect = (end.tv_sec - begin.tv_sec) * 1000000
//					+ (end.tv_usec - begin.tv_usec);
//			printf("\t\t drawmask time : %d\n", time_detect);

			memset(facetemp, 0, sizeof(objPos) * 50);

			//detect face
//			gettimeofday(&begin, NULL);
			fs = kise_detectface(grayimage, width, height,
					widthstep, facetemp, MinSize, MaxSize,
					scale, pf->motioninter);
//			gettimeofday(&end, NULL);
//			time_detect = (end.tv_sec - begin.tv_sec) * 1000000
//					+ (end.tv_usec - begin.tv_usec);
//			printf("\t\t kise_detectface time : %d\n", time_detect);
			for (i = 0; i < fs; i++) {
				RECT r;
				float similarity = 0.1;

				if (facetemp[i].width == 0
						|| facetemp[i].height == 0) {
					continue;
				}
				r.left = facetemp[i].x;
				r.top = facetemp[i].y;
				r.right = facetemp[i].x + facetemp[i].width;
				r.bottom = facetemp[i].y + facetemp[i].height;

//				gettimeofday(&begin, NULL);

				similarity = kzTempmatch(grayimage,
						pf->background8, r, widthstep,
						width);

//				gettimeofday(&end, NULL);
//				time_detect = (end.tv_sec - begin.tv_sec)
//						* 1000000
//						+ (end.tv_usec - begin.tv_usec);
//				printf("\t\t kzTempmatch time : %d\n",
//						time_detect);

				//similarity with background then delete this face
				if (similarity > 0.75) {
					continue;
				}

				face[facecounts].x = facetemp[i].x;
				face[facecounts].y = facetemp[i].y;
				face[facecounts].width = facetemp[i].width;
				face[facecounts].height = facetemp[i].height;

				facecounts++;

				if (facecounts >= 50) {
					break;
				}
			}
		}
	}

	frame++;
	return facecounts;
}
